

/*-------------------------------------------------------
                 Headerfiles 
---------------------------------------------------------*/
#include <REG167.H>
#include "uart.h"
#include "Decoder.h"
#include "Protokoll.h"
#include "mem_access.h"

char retBuf[256];
int retPos = 0;

void Decode( char* buf)
{
	int bufPos = 0;

	char length = 0;
	long srcAdr = 0;
	long destAdr = 0;
	unsigned long readValue = 0;

	int i = 0;
	int errorFlag = 0;
	
	// Reset Values
	retPos = 0;
	
	switch(buf[0] & 0x0F) 
	{ // Function Select, lower byte
	
		// COM TEST
		case 1:
			if( (buf[1] == (char)0xAA) && (buf[2] == 0x55) )
			{
				// Send Response
				retBuf[retPos] = 0x55;
				retPos += 1;
				
				retBuf[retPos] = (char)0xAA;
				retPos += 1;
				NewProtokollPart(retBuf, retPos);
			}
			else 
			{
			
			}
		
		break;
		
		// READ
		case 2:		
			// get srcAddress
			srcAdr = (((long)buf[3])<<16);
			srcAdr |= (((long)buf[2])<<8);
			srcAdr |= (((long)buf[1]));
		
			// get Length
			length = buf[4];
			
			prepareResponseHeader(&buf[1], length);
			
			// Reading Loop			
			for(i = 0; i < length; i++) 
			{
				// Call Read Memory Fnc
				readValue = read_mem(srcAdr);  // TODO: check for signed/unsigned!
				srcAdr += 2;
				i++;
				if(readValue < ERROR) 
				{
					// OK
					retBuf[retPos] = (char)(readValue&0x000000FF);
					retPos += 1;
					retBuf[retPos] = (char)((readValue>>8)&0x000000FF);
					retPos += 1;
				}
				else 
				{
					errorFlag = 1;				
				}
			}
			
			// Send Response 1
			if(errorFlag == 1) { // ERROR
				// TODO
			}
			else {	// OK
				NewProtokollPart(retBuf, retPos);
			}
		
		break;
		
		// WRITE
		case 3:
			// get Dest Adr
			destAdr = (((long)buf[3])<<16);
			destAdr |= (((long)buf[2])<<8);
			destAdr |= (((long)buf[1]));
		
			// get Length
			length = buf[4];
			
			// get Src Adr = Pointer pos of DataArray
			
			// Call write mem
			if( write_mem(destAdr,&buf[5],length) < ERROR) 
			{
				// OK
				prepareResponseHeader(&buf[1], length);	
				
				// Send Response
				NewProtokollPart(retBuf, retPos);
			
			}
			else 
			{
			// ERROR
			
			}
		
		break;
		
		// ERROR
		default:
		
		break;
	}
}

void prepareResponseHeader(char* startAdr, int length) 
{			
	// Prepare Response : StartAddress & Length
	retBuf[retPos++] = startAdr[0];			
	retBuf[retPos++] = startAdr[1];
	retBuf[retPos++] = startAdr[2];
			
	retBuf[retPos++] = (char)length;
}
